﻿#ifndef QUATERNION
#define QUATERNION

    // 四元数运算
    #define quaternion float4
    #define matrix float4x4

    #ifndef PI
        #define PI            3.14159265359
    #endif

    #ifndef TWO_PI
        #define TWO_PI        6.28318530718
    #endif
    
    #ifndef HALF_PI
        #define HALF_PI       1.57079632679
    #endif

    #ifndef INV_PI
        #define INV_PI        0.31830988618
    #endif

    #ifndef INV_TWO_PI
        #define INV_TWO_PI    0.15915494309
    #endif

    #define Mul(a,b) Qmul(a,b)

    quaternion AngleAxis(float3 axis, float angle)
    {
        float s,c;
        sincos(angle * PI / 360.0, s, c);
        return quaternion(axis * s, c);
    }

    quaternion RadAxis(float3 axis, float radValue)
    {
        float s,c;
        sincos(radValue * 0.5, s, c);
        return quaternion(axis * s, c);
    }

    quaternion Qmul(quaternion lhs, quaternion rhs)
    {
        return quaternion(lhs.w * rhs.x + lhs.x * rhs.w + lhs.y * rhs.z - lhs.z * rhs.y, 
            lhs.w * rhs.y + lhs.y * rhs.w + lhs.z * rhs.x - lhs.x * rhs.z, 
            lhs.w * rhs.z + lhs.z * rhs.w + lhs.x * rhs.y - lhs.y * rhs.x, 
            lhs.w * rhs.w - lhs.x * rhs.x - lhs.y * rhs.y - lhs.z * rhs.z);
    }

    float3 Qmul(quaternion rotation, float3 pos)
    {
        float num = rotation.x * 2.0;
        float num2 = rotation.y * 2.0;
        float num3 = rotation.z * 2.0;
        float num4 = rotation.x * num;
        float num5 = rotation.y * num2;
        float num6 = rotation.z * num3;
        float num7 = rotation.x * num2;
        float num8 = rotation.x * num3;
        float num9 = rotation.y * num3;
        float num10 = rotation.w * num;
        float num11 = rotation.w * num2;
        float num12 = rotation.w * num3;
        float3 result;
        result.x = (1.0 - (num5 + num6)) * pos.x + (num7 - num12) * pos.y + (num8 + num11) * pos.z;
        result.y = (num7 + num12) * pos.x + (1.0 - (num4 + num6)) * pos.y + (num9 - num10) * pos.z;
        result.z = (num8 - num11) * pos.x + (num9 + num10) * pos.y + (1.0 - (num4 + num5)) * pos.z;
        return result;
    }

    quaternion Euler(float3 angles)
    {
        float angleToRad = PI / 360;
        float s,c;
        quaternion qx,qy,qz;
        sincos(angles.x * angleToRad, s, c);
        qx = quaternion(float3(s,0,0), c);
        sincos(angles.y * angleToRad, s, c);
        qy = quaternion(float3(0,s,0), c);
        sincos(angles.z * angleToRad, s, c);
        qz = quaternion(float3(0,0,s), c);
        quaternion q = Qmul(qx, qz);
        q = Qmul(qy, q);
        return q;
    }

    float4x4 GetRotationMatrix(quaternion q)
    {
        float num = q.x * 2.0;
        float num2 = q.y * 2.0;
        float num3 = q.z * 2.0;
        float num4 = q.x * num;
        float num5 = q.y * num2;
        float num6 = q.z * num3;
        float num7 = q.x * num2;
        float num8 = q.x * num3;
        float num9 = q.y * num3;
        float num10 = q.w * num;
        float num11 = q.w * num2;
        float num12 = q.w * num3;
        float4x4 result;
        result._m00 = 1.0 - (num5 + num6);
        result._m10 = num7 + num12;
        result._m20 = num8 - num11;
        result._m30 = 0.0;
        result._m01 = num7 - num12;
        result._m11 = 1.0 - (num4 + num6);
        result._m21 = num9 + num10;
        result._m31 = 0.0;
        result._m02 = num8 + num11;
        result._m12 = num9 - num10;
        result._m22 = 1.0 - (num4 + num5);
        result._m32 = 0.0;
        result._m03 = 0.0;
        result._m13 = 0.0;
        result._m23 = 0.0;
        result._m33 = 1.0;
        return result;
    }

    #define RotateVector_half(vector,angles,result) result = Qmul(Euler(angles),vector); 
    #define RotateVector_float(vector,angles,result) result = Qmul(Euler(angles),vector);

#endif